#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/shared_array.hpp>
#include <math.h>
#include <iostream>
#include <sstream>
using boost::shared_ptr;
using boost::shared_array;
using std::vector;
using std::cerr;
using std::cout;
using std::string;
using std::ostringstream;



/* version 3.56c. (c) Copyright 1993 by Joseph Felsenstein.
   Written by Joseph Felsenstein, Akiko Fuseki, Sean Lamont, and Andrew Keeffe.
   Permission is granted to copy and use this program provided no fee is
   charged for it and provided that this copyright notice is not removed. */

#define epsilon         0.000001   /* a very small but not too small number */


struct node
{		  /* describes a tip species or an ancestor */
	node(unsigned count) : next (0), back(0), index(0), tip(false), d(count, 0.0),
						w(count, 1.0), t(0.0), sametime(false), weight(0.0), processed(false)
	{
	}
	node *next;
	node *back;
	long index;
	bool tip;				/* present species are tips of tree       */
	vector<double> d;
	vector<double> w;	/* distances and weights                  */
	double t;				/* time                                   */
	bool sametime;			/* bookkeeps scrunched nodes              */
	double weight;			/* weight of node used by scrunch         */
	bool processed;			/* used by evaluate                       */
};

// globals
namespace{

	vector<shared_ptr<node> >	shared_mem;
	vector<node* > 				treenode, bestree;
	vector<long>				enterorder;
	node*	root;
	node*	best;
	// numsp = number of species
	long	numsp;
	long	numsp2;
	long	i;
	long	j;
	long	l;
	long	jumb;
	
	long	njumble = 200;
	double	power = 2.0;
	long	examined;
	double	like;
	double	bestyet;
	node *	there;
}









//________________________________________________________________________________________

//	doinit

//________________________________________________________________________________________
void doinit()
{
	/* initializes variables */
	long i, j;
	node *p, *q;

	treenode.insert(treenode.begin(), numsp2, 0);
	for (i = 0; i < (numsp); i++)
	{
		shared_mem.push_back(shared_ptr<node>(new node(numsp2)));
		treenode[i] = shared_mem.back().get();
	}
	for (i = numsp; i < (numsp2); i++)
	{
		q = NULL;
		for (j = 1; j <= 3; j++)
		{
			shared_mem.push_back(shared_ptr<node>(new node(numsp2)));
			p = shared_mem.back().get();
			p->next = q;
			q = p;
		}
		p->next->next->next = p;
		treenode[i] = p;
	}
	if (njumble > 1)
	{
		bestree.insert(treenode.begin(), numsp2, 0);
		for (i = 0; i < (numsp); i++)
		{
			shared_mem.push_back(shared_ptr<node>(new node(numsp2)));
			bestree[i] = shared_mem.back().get();
		}
		for (i = numsp; i < (numsp2); i++)
		{
			q = NULL;
			for (j = 1; j <= 3; j++)
			{
				shared_mem.push_back(shared_ptr<node>(new node(numsp2)));
				p = shared_mem.back().get();
				p->next = q;
				q = p;
			}
			p->next->next->next = p;
			bestree[i] = p;
		}
	}

}  /* doinit */






void assigndata(std::vector<boost::shared_array<double> >& matrix)
{
	for (unsigned i = 1; i <= (numsp2); i++)
	{
		treenode[i - 1]->back = NULL;
		treenode[i - 1]->index = i;
		treenode[i - 1]->tip = (i <= numsp);
		treenode[i - 1]->t = 0.0;
		treenode[i - 1]->sametime = false;
		if (i > numsp)
		{
			node* p = treenode[i - 1]->next;
			while (p != treenode[i - 1])
			{
				p->back = NULL;
				p->tip = false;
				p->index = i;
				p = p->next;
			}
		}
	}
	if (njumble > 1)
		for (unsigned i = 1; i <= (numsp2); i++)
		{
			bestree[i - 1]->back = NULL;
			bestree[i - 1]->index = i;
			bestree[i - 1]->tip = (i <= numsp);
			bestree[i - 1]->t = 0.0;
			bestree[i - 1]->sametime = false;
			if (i > numsp)
			{
				node* p = bestree[i - 1]->next;
				while (p != bestree[i - 1])
				{
					p->back = NULL;
					p->tip = false;
					p->index = i;
					p = p->next;
				}
			}
		}
	
		
		
		
	for (unsigned i = 0; i < numsp - 1 ; ++i)
		for (unsigned j = i + 1; j < numsp; ++j)
		{
			treenode[i]->d[j] = treenode[j]->d[i] = matrix[i][j];
			if (matrix[i][j] > 10000)
				treenode[i]->w[j] = treenode[j]->w[i] = 0;
			else
				treenode[i]->w[j] = treenode[j]->w[i] =
													1.0 / exp(2.0 * log(matrix[i][j]));
		}
	
	
	
}  /* getdata */




//________________________________________________________________________________________

//	add

//________________________________________________________________________________________
void add(node* below, node* newtip, node* newfork)
{
	/* inserts the nodes newfork and its left descendant, newtip,
	   to the tree.  below becomes newfork's right descendant */
	if (below != treenode[below->index - 1])
		below = treenode[below->index - 1];
	if (below->back != NULL)
		below->back->back = newfork;
	newfork->back = below->back;
	below->back = newfork->next->next;
	newfork->next->next->back = below;
	newfork->next->back = newtip;
	newtip->back = newfork->next;
	if (root == below)
		root = newfork;
	root->back = NULL;
}  /* add */




//________________________________________________________________________________________

//	re_move

//________________________________________________________________________________________
void re_move(node ** item, node** fork)
{
	/* removes nodes item and its ancestor, fork, from the tree.
	   the new descendant of fork's ancestor is made to be
	   fork's second descendant (other than item).  Also
	   returns pointers to the deleted nodes, item and fork */
	node *p, *q;

	if ((*item)->back == NULL)
	{
		*fork = NULL;
		return;
	}
	*fork = treenode[(*item)->back->index - 1];
	if (root == *fork)
	{
		if (*item == (*fork)->next->back)
			root = (*fork)->next->next->back;
		else
			root = (*fork)->next->back;
	}
	p = (*item)->back->next->back;
	q = (*item)->back->next->next->back;
	if (p != NULL)
		p->back = q;
	if (q != NULL)
		q->back = p;
	(*fork)->back = NULL;
	p = (*fork)->next;
	while (p != *fork)
	{
		p->back = NULL;
		p = p->next;
	}
	(*item)->back = NULL;
}  /* remove */




//________________________________________________________________________________________

//	scrunchtraverse

//________________________________________________________________________________________
void scrunchtraverse(node *u, node** closest, double *tmax)
{
	/* traverse to find closest node to the current one */
	if (!u->sametime)
	{
		if (u->t > *tmax)
		{
			*closest = u;
			*tmax = u->t;
		}
		return;
	}
	u->t = treenode[u->back->index - 1]->t;
	if (!u->tip)
	{
		scrunchtraverse(u->next->back, closest,tmax);
		scrunchtraverse(u->next->next->back, closest,tmax);
	}
}  /* scrunchtraverse */




//________________________________________________________________________________________

//	combine

//________________________________________________________________________________________
void combine(node* a, node* b)
{
	/* put node b into the set having the same time as a */
	if (a->weight + b->weight <= 0.0)
		a->t = 0.0;
	else
		a->t = (a->t * a->weight + b->t * b->weight) / (a->weight + b->weight);
	a->weight += b->weight;
	b->sametime = true;
}  /* combine */




//________________________________________________________________________________________

//	scrunch

//________________________________________________________________________________________
void scrunch(node *s)
{
	/* see if nodes can be combined to prevent negative lengths */
/* Local variables for scrunch: */
	double tmax;
	node *closest;
	bool found;

	closest = NULL;
	tmax = -1.0;
	do
	{
		if (!s->tip)
		{
			scrunchtraverse(s->next->back, &closest,&tmax);
			scrunchtraverse(s->next->next->back, &closest,&tmax);
		}
		found = (tmax > s->t);
		if (found)
			combine(s, closest);
		tmax = -1.0;
	} while (found);
}  /* scrunch */




//________________________________________________________________________________________

//	secondtraverse

//________________________________________________________________________________________
void secondtraverse(node *a, node* q, node* u, node* v, long i, long j, long k, double *sum)
{
	/* recalculate distances, add to sum */
	long l;
	double wil, wjl, wkl, wli, wlj, wlk, TEMP;

	if (!(a->processed || a->tip))
	{
		secondtraverse(a->next->back, q,u,v,i,j,k,sum);
		secondtraverse(a->next->next->back, q,u,v,i,j,k,sum);
		return;
	}
	if (!(a != q && a->processed))
		return;
	l = a->index;
	wil = u->w[l - 1];
	wjl = v->w[l - 1];
	wkl = wil + wjl;
	wli = a->w[i - 1];
	wlj = a->w[j - 1];
	wlk = wli + wlj;
	q->w[l - 1] = wkl;
	a->w[k - 1] = wlk;
	if (wkl <= 0.0)
		q->d[l - 1] = 0.0;
	else
		q->d[l - 1]	= (wil * u->d[l - 1] + wjl * v->d[l - 1]) / wkl;
	if (wlk <= 0.0)
		a->d[k - 1] = 0.0;
	else
		a->d[k - 1]	= (wli * a->d[i - 1] + wlj * a->d[j - 1]) / wlk;
	if (wkl > 0.0)
	{
		TEMP = u->d[l - 1] - v->d[l - 1];
		(*sum) += wil * wjl / wkl * (TEMP * TEMP);
	}
	if (wlk > 0.0)
	{
		TEMP = a->d[i - 1] - a->d[j - 1];
		(*sum) += wli * wlj / wlk * (TEMP * TEMP);
	}
}  /* secondtraverse */




//________________________________________________________________________________________

//	firstraverse

//________________________________________________________________________________________
void firstraverse(node *q_, node* r, double *sum)
{  /* firsttraverse                              */
   /* go through tree calculating branch lengths */
	/* Local variables for firstraverse: */
	node *q;
	long i, j, k;
	node *u, *v;

	q = q_;
	if (q == NULL)
		return;
	q->sametime = false;
	if (!q->tip)
	{
		firstraverse(q->next->back, r,sum);
		firstraverse(q->next->next->back, r,sum);
	}
	q->processed = true;
	if (q->tip)
		return;
	u = q->next->back;
	v = q->next->next->back;
	i = u->index;
	j = v->index;
	k = q->index;
	if (u->w[j - 1] + v->w[i - 1] <= 0.0)
		q->t = 0.0;
	else
		q->t = (u->w[j - 1] * u->d[j - 1] +
				v->w[i - 1] * v->d[i - 1]) /
			   (2.0 * (u->w[j - 1] + v->w[i - 1]));
	q->weight = u->weight + v->weight + u->w[j - 1] + v->w[i - 1];
//	if (!negallowed)
   	scrunch(q);
	secondtraverse(r,q,u,v,i,j,k,sum);
}  /* firstraverse */




//________________________________________________________________________________________

//	sumtraverse

//________________________________________________________________________________________
void sumtraverse(node *q, double *sum)
{
	/* traverse to finish computation of sum of squares */
	long i, j;
	node *u, *v;
	double TEMP, TEMP1;

	if (q->tip)
		return;
	sumtraverse(q->next->back, sum);
	sumtraverse(q->next->next->back, sum);
	u = q->next->back;
	v = q->next->next->back;
	i = u->index;
	j = v->index;
	TEMP = u->d[j - 1] - 2.0 * q->t;
	TEMP1 = v->d[i - 1] - 2.0 * q->t;
	(*sum) += u->w[j - 1] * (TEMP * TEMP) + v->w[i - 1] * (TEMP1 * TEMP1);
}  /* sumtraverse */




//________________________________________________________________________________________

//	evaluate

//________________________________________________________________________________________
void evaluate(node *r)
{
	/* fill in times and evaluate sum of squares for tree */
	/* Local variables for evaluate: */
	double sum;
	long i;
	sum = 0.0;
	for (i = 0; i < (numsp2); i++)
		treenode[i]->processed = treenode[i]->tip;
	firstraverse(r, r,&sum);
	sumtraverse(r, &sum);
	examined++;
//	if (replicates && (lower || upper))
//		sum /= 2;
	like = -sum;
}  /* evaluate */





//________________________________________________________________________________________

//	tryadd

//________________________________________________________________________________________
void tryadd(node *p,  node** item,  node** nufork)
{
	/* temporarily adds one fork and one tip to the tree.
	   if the location where they are added yields greater
	   "likelihood" than other locations tested up to that
	   time, then keeps that location as there */
	add(p, *item, *nufork);
	evaluate(root);
	if (like > bestyet)
	{
		bestyet = like;
		there = p;
	}
	re_move(item, nufork);
}  /* tryadd */




//________________________________________________________________________________________

//	addpreorder

//________________________________________________________________________________________
void addpreorder(node *p, node* item, node* nufork)
{
	/* traverses a binary tree, calling PROCEDURE tryadd
	   at a node before calling tryadd at its descendants */
/* Local variables for addpreorder: */
	if (p == NULL)
		return;
	tryadd(p, &item,&nufork);
	if (!p->tip)
	{
		addpreorder(p->next->back, item, nufork);
		addpreorder(p->next->next->back, item, nufork);
	}
}  /* addpreorder */




//________________________________________________________________________________________

//	tryrearr

//________________________________________________________________________________________
void tryrearr(node *p,  node** r, bool *success)
{
	/* evaluates one rearrangement of the tree.
	   if the new tree has greater "likelihood" than the old
	   one sets success := TRUE and keeps the new tree.
	   otherwise, restores the old tree */
	node *frombelow, *whereto, *forknode;
	double oldlike;

	if (p->back == NULL)
		return;
	forknode = treenode[p->back->index - 1];
	if (forknode->back == NULL)
		return;
	oldlike = like;
	if (p->back->next->next == forknode)
		frombelow = forknode->next->next->back;
	else
		frombelow = forknode->next->back;
	whereto = forknode->back;
	re_move(&p, &forknode);
	add(whereto, p, forknode);
	if ((*r)->back != NULL)
		*r = treenode[(*r)->back->index - 1];
	evaluate(*r);
	if (like > oldlike)
	{
		bestyet = like;
		*success = true;
		return;
	}
	re_move(&p, &forknode);
	add(frombelow, p, forknode);
	if ((*r)->back != NULL)
		*r = treenode[(*r)->back->index - 1];
	like = oldlike;
}  /* tryrearr */




//________________________________________________________________________________________

//	repreorder

//________________________________________________________________________________________
void repreorder(node *p,  node** r, bool *success)
{
	/* traverses a binary tree, calling PROCEDURE tryrearr
	   at a node before calling tryrearr at its descendants */
	if (p == NULL)
		return;
	tryrearr(p,r,success);
	if (!p->tip)
	{
		repreorder(p->next->back,r,success);
		repreorder(p->next->next->back,r,success);
	}
}  /* repreorder */




//________________________________________________________________________________________

//	rearrange

//________________________________________________________________________________________
void rearrange(node ** r_)
{
	/* traverses the tree (preorder), finding any local
	   rearrangement which decreases the number of steps.
	   if traversal succeeds in increasing the tree's
	   "likelihood", PROCEDURE rearrange runs traversal again */
/* Local variables for rearrange: */
	node **r;
	bool success;
	r = r_;
	success = true;
	while (success)
	{
		success = false;
		repreorder(*r,r,&success);
	}
}  /* rearrange */





//________________________________________________________________________________________

//	treeout

//________________________________________________________________________________________
void treeout(node *p, vector<string>& names, std::ostream& tree_strm)
{
	/* write out file with representation of final tree */
	if (p->tip)
		tree_strm << names[p->index - 1];
	else
	{
		tree_strm << "(";
		treeout(p->next->back, names, tree_strm);
		tree_strm << ",";
		treeout(p->next->next->back, names, tree_strm);
		tree_strm << ")";
	}
	if (p == root)
		tree_strm << ";\n";
	else
		tree_strm << ":" << (treenode[p->back->index - 1]->t - p->t);
}  /* treeout */




//________________________________________________________________________________________

//	dtraverse

//________________________________________________________________________________________
/*void dtraverse(node *q)
{
	// print table of lengths etc.
	long i;

	if (!q->tip)
		dtraverse(q->next->back);
	if (q->back != NULL)
	{
		fprintf(outfile, "%4ld  ", q->back->index - numsp);
		if (q->index <= numsp)
		{
			for (i = 0; i < namelength; i++)
				putc(names[q->index - 1][i], outfile);
		}
		else
			fprintf(outfile, "%4ld      ", q->index - numsp);
		fprintf(outfile, "%13.5f", treenode[q->back->index - 1]->t - q->t);
		fprintf(outfile, "%15.5f\n", root->t - q->t);
	}
	if (!q->tip)
		dtraverse(q->next->next->back);
}  /* dtraverse */




//________________________________________________________________________________________

//	describe

//________________________________________________________________________________________
void describe(double& sum_sqrs, double& avg_percent_stddev)
{
	/* prints table of lengths, times, sum of squares, etc. */
	long i, j;
	double totalnum;

//	fprintf(outfile, "\nSum of squares = %10.3f\n\n", -like);
	sum_sqrs = -like;
	if (fabs(power - 2) < 0.01)
	{
		totalnum = 0.0;
		for (i = 0; i < (numsp); i++)
		{
			for (j = 0; j < (numsp); j++)
				if (i + 1 != j + 1 && treenode[i]->d[j] > 0.0)
					totalnum += treenode[i]->w[j] * treenode[i]->d[j] * treenode[i]->d[j];
		}
		totalnum -= 2;
//		if (replicates && (lower || upper))
//			totalnum /= 2;
//		fprintf(outfile, "Average percent standard deviation =");
//		fprintf(outfile, "%10.5f\n\n", 100 * sqrt(-(like / totalnum)));
		avg_percent_stddev = 100 * sqrt(-(like / totalnum));
	}
//	fprintf(outfile, "examined %4ld trees\n\n", examined);
//	fprintf(outfile, "From    To           Length          Time\n");
//	fprintf(outfile, "----    --           ------          ----\n\n");
//	dtraverse(root);
//	putc('\n', outfile);
}  /* describe */





//________________________________________________________________________________________

//	copynode

//________________________________________________________________________________________
void copynode(node *c,  node* d)
{
	/* make a copy of a node */

//	memcpy(d->d, c->d, numsp2*sizeof(double));
//	memcpy(d->w, c->w, numsp2*sizeof(double));
	d->d = c->d;
	d->w = c->w;
	d->t = c->t;
	d->sametime = c->sametime;
	d->weight = c->weight;
	d->processed = c->processed;
}  /* copynode */




//________________________________________________________________________________________

//	copy_

//________________________________________________________________________________________
void copy_(vector<node*> a, vector<node*> b)
{
	/* make a copy of a tree */
	short i, j=0;
	node *p, *q;

	for (i = 0; i < numsp; i++)
	{
		copynode(a[i], b[i]);
		if (a[i]->back != NULL)
		{
			if (a[i]->back == a[a[i]->back->index - 1])
				b[i]->back = b[a[i]->back->index - 1];
			else if (a[i]->back == a[a[i]->back->index - 1]->next)
				b[i]->back = b[a[i]->back->index - 1]->next;
			else
				b[i]->back = b[a[i]->back->index - 1]->next->next;
		}
		else b[i]->back	= NULL;
	}
	for (i = numsp; i < numsp2; i++)
	{
		p = a[i];
		q = b[i];
		for (j = 1; j <= 3; j++)
		{
			copynode(p, q);
			if (p->back)
			{
				if (p->back == a[p->back->index - 1])
					q->back = b[p->back->index - 1];
				else if (p->back == a[p->back->index - 1]->next)
					q->back = b[p->back->index - 1]->next;
				else
					q->back	= b[p->back->index - 1]->next->next;
			}
			else
				q->back	= NULL;
			p = p->next;
			q = q->next;
		}
	}
}  /* copy */





//________________________________________________________________________________________

//	maketree

//________________________________________________________________________________________
void maketree(std::vector<boost::shared_array<double> >& matrix)
{
	/* constructs a binary tree from the pointers in treenode.
	   adds each node at location which yields highest "likelihood"
	   then rearranges the tree for greatest "likelihood" */
	long i, j, k;
	double bestlike, bstlike2, gotlike;
	bool lastrearr;
	node *item, *nufork;
	if (jumb == 1)
	{
		assigndata(matrix);
		examined = 0;
	}
	for (i = 1; i <= (numsp); i++)
		enterorder[i - 1] = i;
	for (i = 0; i < (numsp); i++)
	{
		j = (rand() % numsp) + 1;
		k = enterorder[j - 1];
		enterorder[j - 1] = enterorder[i];
		enterorder[i] = k;
	}
	root = treenode[enterorder[0] - 1];
	add(treenode[enterorder[0] - 1], treenode[enterorder[1] - 1], treenode[numsp]);
	cerr << ".";
	lastrearr = false;
	for (i = 3; i <= (numsp); i++)
	{
		bestyet = -10000000.0;
		item = treenode[enterorder[i - 1] - 1];
		nufork = treenode[numsp + i - 2];
		addpreorder(root, item, nufork);
		add(there, item, nufork);
		like = bestyet;
		rearrange(&root);
		evaluate(root);
		examined--;
		lastrearr = (i == numsp);
		if (lastrearr)
		{
			bestlike = bestyet;
			do
			{
				gotlike = bestlike;
				for (j = 0; j < (numsp2); j++)
				{
					there = root;
					bestyet = -32000.0;
					item = treenode[j];
					if (item != root)
					{
						re_move(&item, &nufork);
						there = root;
						addpreorder(root, item, nufork);
						add(there, item, nufork);
					}
				}
			} while (bestlike > gotlike);
			if (njumble > 1)
			{
				if (jumb == 1 || (jumb > 1 && bestlike > bstlike2))
				{
					copy_(treenode, bestree);
					best = bestree[root->index -1];
					bstlike2 = bestlike;
				}
			}
		}
		if (i == numsp && njumble == jumb)
		{
			if (njumble > 1)
			{
				copy_(bestree, treenode);
				root = treenode[best->index - 1];
			}
			evaluate(root);
		}
	}
}  /* maketree */


string do_main(std::vector<boost::shared_array<double> >& matrix, vector<string>& names,
				double& sum_sqrs, double& avg_percent_stddev)

{  /* Fitch-Margoliash criterion with contemporary tips */
	numsp = matrix.size();
	numsp2 = numsp * 2 - 1;
	doinit();
	enterorder.insert(enterorder.begin(), numsp, 0);
	for (jumb = 1; jumb <= njumble; jumb++)
		maketree(matrix);
	describe(sum_sqrs, avg_percent_stddev);
	std::ostringstream tree_strm;
	treeout(root, names, tree_strm);
	enterorder.clear();
	shared_mem.clear();
	return tree_strm.str();
	
}  /* Fitch-Margoliash criterion with contemporary tips */



